#include "Detector.h"

Detector::Detector()
{

#ifdef ADJUST
    adjustParam();
#endif;

    Ptr<ml::SVM> svm = ml::SVM::load("/media/dji/139389d4-c58d-476d-89a6-1ddfcb52c403/Project/AutoAim(shaobingxia)/assert/files/cxy_svm_5_1.xml");
    islost = true;
    islost_before = true;
}

Detector::Detector(Mat src0)
{

#ifdef ADJUST
    adjustParam();
#endif

    src0.copyTo(src_out);
    Ptr<ml::SVM> svm = ml::SVM::load("/media/dji/139389d4-c58d-476d-89a6-1ddfcb52c403/Project/AutoAim(shaobingxia)/assert/files/cxy_svm_5_1.xmll");
    islost = true;
    islost_before = true;
}

void Detector::getSrcImage(Mat& src0)
{
    this_time=clock.thisTime();
    //cout<<"_____________________________________________________________________total usetime:"<<this_time-time_before<<"ms"<<endl;
    cout<<"_____________________________________________________________________fps:"<<(float)1.0/(this_time-time_before)<<"ms"<<endl;
    time_before=this_time;
    src_cols=src0.cols;
    src_rows=src0.rows;
    // cout << "target_roi.x=" << target_roi.x << endl;
    // cout << "target_roi.y=" << target_roi.y << endl;
    // cout << "target_roi.w=" << target_roi.width << endl;
    // cout << "target_roi.h=" << target_roi.height << endl;
    // cout << "islost_before=" << islost_before << endl;
    //置空装甲板
    if (detect_armor.size() != 0)
    {
        detect_armor.clear();
    }
#ifdef ADJUST
    src0.copyTo(src);
#endif
    
    src0.copyTo(src_out);
    //灰灭判断器是否处于工作状态
    if (clock.status() == true)
    {
        clock.end();
    }

    if (islost_before == false)
    {
        src_out = src_out(target_roi);
        isLock = true;
        cout << "detected and make a roi" << endl;
    }

    // else if (islost_before == true && this->clock.timeConsuming(SECOND) < GRAY_TIME)
    // {
    //     src_out = src_out(target_roi);
    //     isLock = true;
    //     cout << "isgray but make a roi" << endl;
    // }
    else
    {
        target_roi = Rect(0, 0, src_cols, src_rows);
        roi_move = Point(0, 0);
        src_out = src_out(target_roi);
        isLock=false;
        //cout<<"islost_before="<<islost_before<<endl;
        cout << "lost not make roi" << endl;
    }
    //src_out.copyTo(src_out0);
    //关闭灰灭判断器的两种情况：1.识别到目标，退出判断状态，关闭灰灭判断器 2.掉帧时常超过0.5秒，判定为死车或丢失目标,关闭灰灭判断器

    if ((islost_before == false && this->clock.status() == true) || (this->clock.status() == true && this->clock.timeConsuming() >= GRAY_TIME))
    {
        this->clock.close();
    }
    // Armor_ROI = src0(Rect(target_roi.x, target_roi.y, target_roi.width, target_roi.height));
#ifdef ADJUST
    imshow("src_out", src_out);
#endif
    
}

void Detector::getBinaryImage()
{
    clock1.begin();
    Mat gry;
    src_out.copyTo(gry);
    if (enemyColor == BLUE)
    {
        binary = pointProcess(gry, enemyColor, color_thresh_blue, gray_thresh_blue);
    }
    else
    {
        binary = pointProcess(gry, enemyColor, color_thresh_red, gray_thresh_red);
    }
#ifdef ADJUST
    imshow("binary", binary);
#endif
    clock1.end();
    cout<<"binary_usetine:"<<clock1.timeConsuming(MILLISECOND)<<"ms"<<endl;
}

void Detector::getContours()
{
    clock1.begin();


    vector<Vec4i> hierarchy;
    Point2f rect[4];
    findContours(binary, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE);

    vector<vector<Point>>::iterator itc = contours.begin();
    while (itc != contours.end())
    {
        if (itc->size() < contour_length_limit)
        {
            itc = contours.erase(itc);
        }
        else
        {
            itc++;
        }
    }
    // #ifdef ADJUST
    //      cout << "contours.size()=" << contours.size() << endl;
    //     // cout<<"//////////////all contours size()/////////////////////"<<endl;
    //     for (int i = 0; i < contours.size(); i++)
    //     {
    //         cout<<"contours[i].size()="<<contours[i].size()<<endl;
    //         drawContours(src, contours, i, Scalar(0, 0, 255), 2, 8);
    //     }
    //     //imshow("binary", binary);
    // #endif
    vector<Rect> boundRect(contours.size());
    vector<RotatedRect> box(contours.size());
    num = contours.size();

    for (int i = 0; i < num; i++)
    {
        box[i] = minAreaRect(contours[i]);
    }
#ifdef ADJUST
    for (int i = 0; i < contours.size(); i++)
    {
        Point2f rect_temp[4];
        box[i].points(rect_temp);
        for (int i = 0; i < 4; i++)
        {
            rect_temp[i].x += roi_move.x;
            rect_temp[i].y += roi_move.y;
        }
        for (int j = 0; j < 4; j++)
        {

            line(src, rect_temp[j], rect_temp[(j + 1) % 4], Scalar(0, 165, 255), 2, 8);
            //circle(src, rect_temp[j], 3.5 * (j + 1), Scalar(255, 255, 255), 2, 8);
        }
        ostringstream text1;
        Point text_position1 = box[i].center;
        text_position1 += roi_move;
        text1 << "box[" << i << "].angle=" << box[i].angle;
        putText(src, text1.str(), text_position1, FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
        text1.str("");
        text1 << "box[" << i << "].size.width=" << box[i].size.width;
        text_position1.y += 10;
        putText(src, text1.str(), text_position1, FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
        text1.str("");
        text1 << "box[" << i << "].size.height=" << box[i].size.height;
        text_position1.y += 10;
        putText(src, text1.str(), text_position1, FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
    }
#endif
    memset(matchrank, 0, sizeof(matchrank));
    for (int i = 0; i < num; i++)
    {
        for (int j = i + 1; j < num; j++)
        {
            //条件1:每个灯条的角度
            if ((box[i].size.width > box[i].size.height && box[i].angle > -(90 - light_angle)) || (box[i].size.width < box[i].size.height && box[i].angle < -light_angle))
                matchrank[i][j] -= 1;
            if ((box[j].size.width > box[j].size.height && box[j].angle > -(90 - light_angle)) || (box[j].size.width < box[j].size.height && box[j].angle < -light_angle))
                matchrank[i][j] -= 1;
            //条件2：每个灯条的长宽比
            double longi, shorti, longj, shortj;
            longi = box[i].size.height;
            shorti = box[i].size.width;

            if (longi < shorti)
            {
                double temp = longi;
                longi = shorti;
                shorti = temp;
            }
            longj = box[j].size.height;
            shortj = box[j].size.width;
            if (longj < shortj)
            {
                double temp = longj;
                longj = shortj;
                shortj = temp;
            }

            if ((longi / shorti) <= (float)(longshort_ratio / 10.0f) && (longj / shortj) <= (float)(longshort_ratio / 10.0f))
                matchrank[i][j] -= 20;
            if ((longi / shorti) >= (float)(longshort_ratio / 10.0f) && (longi / shorti) <= 8 && (longj / shortj) >= (float)(longshort_ratio / 10.0f) && (longj / shortj) <= 8)
                matchrank[i][j] += 0;
            // if ((longi / shorti) >= 0.7 && (longi / shorti) <= 1.8 && (longj / shortj) >= 0.7 && (longj / shortj) <= 1.8)
            //     matchrank[i][j] -= 10000;
            // if ((longi / shorti) >= 1.8 && (longi / shorti) <= 8 && (longj / shortj) >= 1.8 && (longj / shortj) <= 8)
            //     matchrank[i][j] += 100;

            //条件3：两灯条中心横纵坐标的差异与两个灯条的长边中较小边比较
            double long_min = min(longi, longj);
            if (((float)(differ_centenrx_min / 10.0f) * long_min < abs(box[i].center.x - box[j].center.x) && abs(box[i].center.x - box[j].center.x) < (float)(differ_centenrx_max / 10.0f) * long_min) && (abs(box[i].center.y - box[j].center.y) < (float)(differ_centenry_max / 10.0f) * long_min))
                matchrank[i][j] += 0;
            else
                matchrank[i][j] -= 300;
            //cout << "(float)(differ_centenrx_min/10.0f)* long_min=" << (float)(differ_centenrx_min / 10.0f) * long_min << endl;
#ifdef ADJUST           
            ostringstream text_3;
            text_3 << "JUDGE3:";
            putText(src, text_3.str(), Point(10, src.rows / 2), FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
            text_3.str("");
            text_3 << "abs(box[" << i << "].center.y - box[" << j << "].center.y)=" << abs(box[i].center.y - box[j].center.y) << endl;
            putText(src, text_3.str(), Point(10, src.rows / 2 + 10), FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
            text_3.str("");
            text_3 << "abs(box[" << i << "].center.x - box[" << j << "].center.x)=" << abs(box[i].center.x - box[j].center.x) << endl;
            putText(src, text_3.str(), Point(10, src.rows / 2 + 20), FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
            text_3.str("");
            text_3 << "long_min=" << long_min;
            putText(src, text_3.str(), Point(10, src.rows / 2 + 30), FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
            text_3.str("");
#endif
            //条件4：两灯条夹角
            double anglei, anglej;
            anglei = box[i].angle;
            anglej = box[j].angle;
            if ((((box[i].size.width < box[i].size.height) && (box[j].size.width < box[j].size.height)) || ((box[i].size.width > box[i].size.height) && (box[j].size.width > box[j].size.height))) && (abs(anglei - anglej) < angle_differ))
                matchrank[i][j] += 0;
            else if ((((box[i].size.width > box[i].size.height) && (box[j].size.width < box[j].size.height)) || ((box[i].size.width < box[i].size.height) && (box[j].size.width > box[j].size.height))) && (abs(anglei - anglej) > (90 - angle_differ)))
                matchrank[i][j] += 0;
            else
                matchrank[i][j] -= 4000;
            // if (abs(anglei - anglej) <= 15 || abs(anglei - anglej) >= 75)
            //     matchrank[i][j] += 100;
            // else
            //     matchrank[i][j] -= 10000;
            // cout<<"abs(anglei - anglej)="<<abs(anglei - anglej)<<endl;

            //条件5：两灯条面积的大比小和最小面积界限
            double areai = box[i].size.area();
            double areaj = box[j].size.area();
            if (areai < areaj)
            {
                double temp = areai;
                areai = areaj;
                areaj = temp;
            }
            if ((areai < area_limit || areaj < area_limit) || (areai / areaj > area_ratio))
                matchrank[i][j] -= 50000;
            else
                matchrank[i][j] += 0;
            //cout<<"maxArea="<<areai<<endl;
            //cout<<"面积比:"<<areai/areaj<<endl;

            //条件6：灯条中心距与最长边比较
            // double d = sqrt((box[i].center.x - box[j].center.x) * (box[i].center.x - box[j].center.x) + (box[i].center.y - box[j].center.y) * (box[i].center.y - box[j].center.y));
            // if (d >= longi * 6 || d >= longj * 6 || d < 2 * longi || d < 2 * longj)
            //     matchrank[i][j] -= 600000;
            // cout<<"灯条中心距："<<d<<endl;
            // cout<<"灯条最长边长:"<<max(longi,longj)<<endl;
        }
    }

#ifdef ADJUST
    //显示筛选二维数组
    ostringstream text2;
    text2 << "matchrank result:" << endl;
    Point text_position2 = Point(10, 10);
    putText(src, text2.str(), text_position2, FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
    for (int i = 0; i < num; i++)
    {
        text2.str("");
        for (int j = 0; j < num; j++)
        {

            text2 << matchrank[i][j] << "                      ";
        }
        text_position2.y += 10;
        putText(src, text2.str(), text_position2, FONT_HERSHEY_SIMPLEX, 0.4, Scalar(0, 255, 0), 1, 8);
    }
#endif



    clock1.end();
    cout<<"contours_usetime:"<<clock1.timeConsuming(MILLISECOND)<<"ms"<<endl;
}

void Detector::getTarget()
{
    clock1.begin();
    int amount = 0;
    int match_num = (num + 1) * num / 2;
    int rect_index[match_num][2];
    memset(rect_index, -1, sizeof(rect_index));
    int temp[2] = {-1};
    int armor_mark[] = {0};
    for (int i = 0; i < num; i++)
    {
        for (int j = i + 1; j < num; j++)
        {
            if (matchrank[i][j] >= 0)
            {
                armor_mark[amount] = matchrank[i][j];
                rect_index[amount][0] = i;
                rect_index[amount][1] = j;
                amount++;
            }
        }
    }

//    cout << "匹配对数=" << amount << endl;
    // imshow("Armor_ROI",Armor_ROI);

    if (rect_index[0][0] == -1 || rect_index[0][1] == -1)
    {
        islost = true;
        if (islost_before == false)
        {
            clock.begin();
        }
        islost_before = islost;
        clock1.end();
        cout<<"lost_target usetime:"<<clock1.timeConsuming(MILLISECOND)<<"ms"<<endl;
        return;
    }

    islost= false;
    RotatedRect boxi;
    RotatedRect boxj;
    int armor_num = 0;
    for (int i = 0; i < amount; i++)
    {
        boxi = minAreaRect(contours[rect_index[i][0]]);
        boxj = minAreaRect(contours[rect_index[i][1]]);

        if (boxi.center.x > boxj.center.x)
        {
            RotatedRect temp;
            temp = boxi;
            boxi = boxj;
            boxj = temp;
        }
        Point2f rect_left[4];
        Point2f rect_right[4];
        boxi.points(rect_left);
        boxj.points(rect_right);

        vector<Point2f> rect_target(4);

        if (boxi.size.width > boxi.size.height)
        {
            rect_target[0] = (rect_left[0] + rect_left[1]) / 2;
            rect_target[1] = (rect_left[2] + rect_left[3]) / 2;
        }
        else
        {
            rect_target[0] = (rect_left[0] + rect_left[3]) / 2;
            rect_target[1] = (rect_left[1] + rect_left[2]) / 2;
        }
        if (boxj.size.width > boxj.size.height)
        {
            rect_target[2] = (rect_right[2] + rect_right[3]) / 2;
            rect_target[3] = (rect_right[0] + rect_right[1]) / 2;
        }
        else
        {
            rect_target[2] = (rect_right[1] + rect_right[2]) / 2;
            rect_target[3] = (rect_right[0] + rect_right[3]) / 2;
        }
#ifdef ADJUST
        // RotatedRect boxx = minAreaRect(rect_target);
        vector<Point2f> rect_target0 = rect_target;
        for (int i = 0; i < 4; i++)
        {
            rect_target0[i].x += roi_move.x;
            rect_target0[i].y += roi_move.y;
        }
        for (int i = 0; i < 4; i++)
        {

            line(src, rect_target0[i], rect_target0[(i + 1) % 4], Scalar(255, 255, 0), 2, 8);
            circle(src, rect_target0[i], 3.5 * (i + 1), Scalar(255, 255, 255), 2, 8);
        }
        // cout<<"rect_target.angle="<<boxx.angle<<endl;
#endif
        int number = 0;
        number = getNumber(rect_target);

        if (number != 0)
        {
            //如果目前有ROI，图像坐标偏移
            // if(roi_move.x!=0&&roi_move.y!=0)
            // {
            //     for(int i=0;i<4;i++)
            //     {
            //         rect_target[i].x+=roi_move.x;
            //         rect_target[i].y+=roi_move.y;
            //     }
            // }

            detect_armor.push_back(Armor(rect_target, roi_move, number));
            armor_num++;
        }
    }

    if (detect_armor.size() != 0)
    {
        //筛选最优装甲板，时间有限先以到相机的距离作为指标，先实现基本功能
        int armor_index = 0;
        for (int i = 1; i < detect_armor.size(); i++)
        {
            if (detect_armor[i].distance < detect_armor[armor_index].distance)
            {
                armor_index = i;
            }
        }
        target_armor = detect_armor[armor_index];

        //getRoi(target_armor.center);
        //  #ifdef ADJUST
        //          for (int i = 0; i < 4; i++)
        //          {
        //              line(src, target_armor.rect[i], target_armor.rect[(i + 1) % 4], Scalar(255, 255, 0), 2, 8);
        //              circle(src, target_armor.rect[i], 2 * (i + 1), Scalar(255, 255, 255), 2, 8);
        //          }
        //          imshow("src", src);
        //  #endif
    }
    else
    {
        islost = true;
    }
    islost_before = islost;
    clock1.end();
    cout<<"get_target usetime:"<<clock1.timeConsuming(MILLISECOND)<<"ms"<<endl;
}

int Detector::getNumber(vector<Point2f> rect_t)
{
    vector<Point> contour;
    contour.push_back(rect_t[0]);
    contour.push_back(rect_t[1]);
    contour.push_back(rect_t[2]);
    contour.push_back(rect_t[3]);
    RotatedRect rect = minAreaRect(contour);
    Point2f center((rect_t[0].x + rect_t[1].x + rect_t[2].x + rect_t[3].x) / 4, (rect_t[0].y + rect_t[1].y + rect_t[2].y + rect_t[3].y) / 4);
    // cout << "angle:" << rect.angle << endl;
    // cout << "rect.size.height=" << rect.size.height << endl;
    // cout << "rect.size.width=" << rect.size.width << endl;
    Mat M;
    // double p1, p2;

    // if (rect.angle < -40)
    // {
    //     rect.angle += 90;
    //     swap(rect.size.width, rect.size.height);
    // }
    //cout << " rect.angle=" << rect.angle << endl;
    // 1280 1024 src.cols src.rows
    if (rect.size.height > rect.size.width)
    {
        rect.angle += 90;
        swap(rect.size.height, rect.size.width);
    }

    M = getRotationMatrix2D(center, rect.angle, 1.0); //求旋转矩阵       顺时针转-angle

    Mat rot_image;
    warpAffine(src_out, rot_image, M, src.size(), 0, 0, 0); //原图像旋转
    // imshow("rot_image", rot_image);
    float x = center.x - rect.size.height, y = center.y - rect.size.height;
    if (x < 0)
        x = 0;
    if (y < 0)
        y = 0;
    float w = (center.x - x) * 2, h = (center.y - y) * 2;
    if (x + w > rot_image.cols)
        w = rot_image.cols - x;
    if (y + h > rot_image.rows)
        h = rot_image.rows - y;

    // cout << "x=" << x << " y=" << y << " h=" << h << " w=" << w << endl;
    number_roi = rot_image(Rect(x, y, w, h)); //提取ROI
    // for (int j = 0; j < 4; j++)
    // {
    // 	line(rot_image, rect_t[j], rect_t[(j + 1) % 4], Scalar(0, 0, 255), 2, 8); //绘制最小外接矩形每条边（非必要）
    // }
    // imshow("rot_image", rot_image);
    // imshow("number_roi", number_roi);
    if (!number_roi.empty())
    {
#ifdef ADJUST
        imshow("number_roi", number_roi);
#endif
        return isArmorPattern(number_roi);
    }

    return 0;
}

int Detector::isArmorPattern(Mat &front)
{
    Mat gray;
    vector<Mat> colorChannel(3);
    split(front, colorChannel);
    //cout << "colorChannel.size()=" << colorChannel.size() << endl;
    // imshow("B",colorChannel.at(0));
    // imshow("G",colorChannel.at(1));
    // imshow("R",colorChannel.at(2));

    cv::cvtColor(front, gray, COLOR_BGR2GRAY);

    cv::resize(gray, gray, Size(20, 20));

    GaussianBlur(gray, gray, Size(3, 3), 0, 0);
    // adaptiveThreshold(gray, gray, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 3, SVM_thresh);
    threshold(gray, gray, SVM_thresh, 255, CV_THRESH_BINARY);
#ifdef ADJUST
    imshow("xxx", gray);
#endif
    //  copy the data to make the matrix continuous

    Mat temp;
    gray.copyTo(temp);
    Mat data = temp.reshape(1, 1);
    // cout<<data<<endl;
    data.convertTo(data, CV_32FC1);

    // cout<<"here!"<<endl;
    //  svm = ml::SVM::load("cxy_svm_5_1.xml");

    //int result = (int)svm->predict(data);

    return 1;
}

void Detector::getRoi(Point2f center)
{
    clock1.begin();
    //目标装甲板灯条长度
    int height = sqrt((target_armor.rect[1].x - target_armor.rect[0].x) * (target_armor.rect[1].x - target_armor.rect[0].x) +
                      (target_armor.rect[1].y - target_armor.rect[0].y) * (target_armor.rect[1].y - target_armor.rect[0].y));
    int x = 0, y = 0, w = 0, h = 0;

    x = center.x - height * 3;
    y = center.y - height * 2;
    w = (center.x - x) * 2;
    h = (center.y - y) * 2;
    if (x < 0)
        x = 0;
    if (y < 0)
        y = 0;
    if (x + w > src_cols)
        w = src_cols - x;
    if (y + h > src_rows)
        h = src_rows - y;
    vector<Point> rect;
    rect.push_back(Point(x, y));
    rect.push_back(Point(x, y + h));
    rect.push_back(Point(x + w, h + y));
    rect.push_back(Point(x + w, y));
#ifdef ADJUST
    for (int i = 0; i < 4; i++)
    {
        line(src, rect.at(i), rect.at((i + 1) % 4), Scalar(0, 255, 0), 2, 8);
    }
    imshow("src", src);
#endif
    // imshow("src_out", src_out);
    // cout << "x=" << x << endl;
    // cout << "w=" << w << endl;
    // cout << "y=" << y << endl;
    // cout << "h=" << h << endl;
    target_roi = Rect(x, y, w, h);
    roi_move = Point(x, y);
    clock1.end();
    cout<<"roi_usetime:"<<clock1.timeConsuming(MILLISECOND)<<"ms"<<endl;
}

void Detector::imgProcess(Mat &tempBinary)
{
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
    erode(tempBinary, tempBinary, kernel);
    dilate(tempBinary, tempBinary, kernel);
}

Mat Detector::pointProcess(Mat srcImg, int enemyColor, int color_thresh, int gray_thresh)
{
    Mat colorBinary;
    Mat gryBinary;

    Mat tempChannels;
    vector<Mat> colorChannels;
    split(srcImg, colorChannels);
    cvtColor(srcImg, gryBinary, COLOR_BGR2GRAY);

    if (enemyColor == RED)
    {
        subtract(colorChannels.at(2), colorChannels.at(0), tempChannels);
        subtract(tempChannels, colorChannels.at(1), colorBinary);
    }
    else
    {
        subtract(colorChannels.at(0), colorChannels.at(2), colorBinary);
    }
    if (enemyColor == RED)
    {
        threshold(colorBinary, colorBinary, color_thresh_red, 255, THRESH_BINARY);
    }
    else
    {
        threshold(colorBinary, colorBinary, color_thresh_blue, 255, THRESH_BINARY);
    }
    imgProcess(colorBinary);

    GaussianBlur(gryBinary, gryBinary, Size(3, 3), 0, 0);
    if (enemyColor == RED)
    {
        threshold(gryBinary, gryBinary, gray_thresh_red, 255, THRESH_BINARY);
    }
    else
    {
        threshold(gryBinary, gryBinary, gray_thresh_blue, 255, THRESH_BINARY);
    }
    return colorBinary & gryBinary;
}

void Detector::adjustParam()
{
    namedWindow("cotorLimit", 1);
    namedWindow("binary", 1);
    createTrackbar("contour_length_limit:", "cotorLimit", &contour_length_limit, 255, NULL);
    createTrackbar("SVM_thresh:", "binary", &SVM_thresh, 255, NULL);
    if (enemyColor == BLUE)
    {
        createTrackbar("color_thresh:", "binary", &color_thresh_blue, 255, NULL);
        createTrackbar("gray_thresh:", "binary", &gray_thresh_blue, 255, NULL);
    }
    else
    {
        createTrackbar("color_thresh:", "binary", &color_thresh_red, 255, NULL);
        createTrackbar("gray_thresh:", "binary", &gray_thresh_red, 255, NULL);
    }
    namedWindow("judge conditon", 1);
    createTrackbar("1.light_angle/10:", "judge conditon", &light_angle, 30, NULL);
    createTrackbar("2.longshort_ratio/10:", "judge conditon", &longshort_ratio, 50, NULL);
    createTrackbar("3.differ_centenrx_min/10:", "judge conditon", &differ_centenrx_min, 50, NULL);
    createTrackbar("3.differ_centenrx_max/10:", "judge conditon", &differ_centenrx_max, 50, NULL);
    createTrackbar("3.differ_centenry_max/10:", "judge conditon", &differ_centenry_max, 20, NULL);
    createTrackbar("4.angle_differ:", "judge conditon", &angle_differ, 20, NULL);
    createTrackbar("5.area_limit:", "judge conditon", &area_limit, 40, NULL);
    createTrackbar("5.area_ratio:", "judge conditon", &area_ratio, 10, NULL);
}

void Detector::getResult(Mat& src0)
{
    
    clock3.begin();
    clock4.begin();
    this->getSrcImage(src0);
    clock4.end();
    //cout<<"_________________________getSrcimage:"<<clock4.timeConsuming(MICROSECOND)<<"ms"<<endl;

    clock4.begin();
    this->getBinaryImage();
    clock4.end();
    //cout<<"_________________________getBinary:"<<clock4.timeConsuming(MICROSECOND)<<"ms"<<endl;

    clock4.begin();
    this->getContours();
    clock4.end();
   // cout<<"_________________________getContours:"<<clock4.timeConsuming(MICROSECOND)<<"ms"<<endl;

    clock4.begin();
    this->getTarget();
    clock4.end();
   // cout<<"_________________________getTarget:"<<clock4.timeConsuming(MICROSECOND)<<"ms"<<endl;
#ifdef ADJUST
    imshow("src", src);
#endif
    clock3.end();
    //cout<<"getResult_usetime:"<<clock3.timeConsuming(MICROSECOND)<<"ms"<<endl;
}